#!/usr/bin/env python

import roslib
roslib.load_manifest('ipm_to_laserscan')
import sys
import rospy
import cv
from std_msgs.msg import String
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError

class video_publisher:

  def __init__(self):
    self.bridge = CvBridge()
    rospy.init_node('video_publisher', anonymous=True)

    self.image_topic = rospy.get_param('~image', "image" )
    self.image_file = rospy.get_param('~file', "./test.avi")
    self.rate = rospy.get_param('~fps', 60)
    self.loop = rospy.get_param('~loop', True)

    self.image_pub = rospy.Publisher(self.image_topic, Image)
    r = rospy.Rate(self.rate)
    print self.image_file

    self.capture = cv.CaptureFromFile(self.image_file)
    rospy.loginfo(str(self.image_file))

    while not rospy.is_shutdown():
        try:
          img = cv.QueryFrame(self.capture)
          if img == None:
             self.capture = cv.CaptureFromFile(self.image_file)
             continue;
          img_msg = self.bridge.cv_to_imgmsg(img, "passthrough")
          self.image_pub.publish(img_msg)
        except CvBridgeError, e:
          rospy.logfatal("You may not be able to play the video: " + self.image_file +"  Please checkit. \nNode died with error " + e)
          rospy.signal_shutdown("Error playing grabing frame or converting to a Ros Image msg.")
        r.sleep()

def main(args):
  ic = video_publisher()
  try:
    rospy.spin()
  except KeyboardInterrupt:
    print "Shutting static movie publisher node"
  cv.DestroyAllWindows()

if __name__ == '__main__':
    main(sys.argv)
